#include <stdio.h>
#include <stdlib.h>
#include <memory.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_pwm.h"
#include "hi_pwm.h"
#include "robot_control.h"

#define GPIO0 0
#define GPIO1 1
#define GPIO9 9
#define GPIO10 10
#define GPIOFUNC 0

// 平衡频率和精度的PWM控制
#define PWM_RESOLUTION 3                       // 4级分辨率 = 250Hz（平衡频率和精度）
#define SPEED_TRANSITION_STEP 1                 // 速度过渡步长

// 通过查询机器人板硬件原理图可知，机器人板上电机驱动芯片使用的是L9110S
// 此器件能够驱动一个直流无刷电机，集成了电机正转/反转/停止/刹车四个功能。
// 左右两轮电机各由一个L9110S驱动,GPOI0和GPIO1控制左轮,GPIO9和GPIO10控制右轮。通过输入GPIO的电平高低控制车轮正转/反转/停止/刹车

// 连续PWM速度控制相关全局变量
static unsigned char g_motor_speed_level = 0;       // 当前电机速度等级(0-100)，从0开始
static unsigned int g_pwm_counter = 0;              // PWM计数器
unsigned char g_left_motor_enable = 0;              // 左电机使能标志（全局可访问）
unsigned char g_right_motor_enable = 0;             // 右电机使能标志（全局可访问）
unsigned char g_left_motor_direction = 0;           // 左电机方向(0=正转,1=反转)（全局可访问）
unsigned char g_right_motor_direction = 0;          // 右电机方向(0=正转,1=反转)（全局可访问）

// 真正的顺滑PWM控制：动态占空比调整
// 核心思想：不是固定占空比，而是渐变过渡
// 等级0: 0%占空比（停止）
// 等级1: 目标30%占空比，但会从当前值渐变到30%
// 等级2: 目标50%占空比，但会从当前值渐变到50%
// 等级3: 目标70%占空比，但会从当前值渐变到70%
// 等级4: 目标90%占空比，但会从当前值渐变到90%
// 等级5: 目标100%占空比，但会从当前值渐变到100%
static unsigned char g_target_duty_cycles[6] = {0, 30, 50, 70, 90, 100};  // 各等级的目标占空比
static unsigned char g_current_pwm_duty = 0;                              // 当前实际占空比
static unsigned char g_transition_step = 1;                               // 每次调整的步长

// gpio_control用于控制GPIO引脚的输出电平值
void gpio_control (unsigned int  gpio, IotGpioValue value) {
    hi_io_set_func(gpio, GPIOFUNC);
    IoTGpioSetDir(gpio, IOT_GPIO_DIR_OUT);//将GPIO引脚设置为输出方向，表示该引脚将用作输出信号的发送端。在输出模式下，可以通过控制GPIO引脚的输出电平值
    IoTGpioSetOutputVal(gpio, value);//设置GPIO引脚的输出电平值。
}

// 设置电机速度等级(0-100) - 连续PWM控制
void set_motor_speed_level(unsigned char speed_level) {
    extern unsigned char g_target_duty_cycle;

    if (speed_level <= 100) {
        g_motor_speed_level = speed_level;
        g_target_duty_cycle = speed_level;  // 设置目标占空比
        printf("Motor speed set to: %d%% (PWM duty cycle: %d)\n", speed_level, g_target_duty_cycle);
    }
}

// 获取当前电机速度等级
unsigned char get_motor_speed_level(void) {
    extern unsigned char g_current_speed_level;  // 使用全局的当前速度等级
    return g_current_speed_level;
}

// 顺滑PWM电机控制函数 - 实现真正的顺滑速度控制
void motor_pwm_control(void) {
    extern unsigned char g_current_level_index;   // 当前速度等级索引
    extern unsigned char g_speed_transition_enabled; // 平滑过渡使能

    g_pwm_counter++;
    if (g_pwm_counter >= PWM_RESOLUTION) {
        g_pwm_counter = 0;
    }

    // 获取目标占空比，并转换为4级PWM
    unsigned char target_duty_100 = g_target_duty_cycles[g_current_level_index];
    unsigned char target_duty = (target_duty_100 * PWM_RESOLUTION) / 100;  // 转换为4级

    // 实现顺滑过渡：当前占空比渐变到目标占空比
    if (g_current_pwm_duty < target_duty) {
        // 加速：逐渐增加占空比
        g_current_pwm_duty += g_transition_step;
        if (g_current_pwm_duty > target_duty) {
            g_current_pwm_duty = target_duty;
        }
    } else if (g_current_pwm_duty > target_duty) {
        // 减速：逐渐减少占空比
        g_current_pwm_duty -= g_transition_step;
        if (g_current_pwm_duty < target_duty) {
            g_current_pwm_duty = target_duty;
        }
    }

    // 平衡PWM控制：4级分辨率，250Hz PWM频率
    unsigned char pwm_high_phase = (g_pwm_counter < g_current_pwm_duty) ? 1 : 0;

    // 简化的高频PWM控制逻辑
    // 核心思想：使用高频PWM（50Hz），让间断感不明显

    // 左电机控制
    if (g_left_motor_enable && g_current_level_index > 0) {
        if (pwm_high_phase) {
            // PWM高电平期间：电机运行
            if (g_left_motor_direction == 0) {
                // 正转：GPIO0低，GPIO1高（修复后）
                gpio_control(GPIO0, IOT_GPIO_VALUE0);
                gpio_control(GPIO1, IOT_GPIO_VALUE1);
            } else {
                // 反转：GPIO0高，GPIO1低（修复后）
                gpio_control(GPIO0, IOT_GPIO_VALUE1);
                gpio_control(GPIO1, IOT_GPIO_VALUE0);
            }
        } else {
            // PWM低电平期间：电机制动（高频切换让用户感觉不到）
            gpio_control(GPIO0, IOT_GPIO_VALUE0);
            gpio_control(GPIO1, IOT_GPIO_VALUE0);
        }
    } else {
        // 完全停止：两个GPIO都为低
        gpio_control(GPIO0, IOT_GPIO_VALUE0);
        gpio_control(GPIO1, IOT_GPIO_VALUE0);
    }

    // 右电机控制
    if (g_right_motor_enable && g_current_level_index > 0) {
        if (pwm_high_phase) {
            // PWM高电平期间：电机运行
            if (g_right_motor_direction == 0) {
                // 正转：GPIO9高，GPIO10低
                gpio_control(GPIO9, IOT_GPIO_VALUE1);
                gpio_control(GPIO10, IOT_GPIO_VALUE0);
            } else {
                // 反转：GPIO9低，GPIO10高
                gpio_control(GPIO9, IOT_GPIO_VALUE0);
                gpio_control(GPIO10, IOT_GPIO_VALUE1);
            }
        } else {
            // PWM低电平期间：电机制动（高频切换让用户感觉不到）
            gpio_control(GPIO9, IOT_GPIO_VALUE0);
            gpio_control(GPIO10, IOT_GPIO_VALUE0);
        }
    } else {
        // 完全停止：两个GPIO都为低
        gpio_control(GPIO9, IOT_GPIO_VALUE0);
        gpio_control(GPIO10, IOT_GPIO_VALUE0);
    }

    // 调试输出（可选）
    static unsigned int debug_counter = 0;
    debug_counter++;
    if (debug_counter % 1000 == 0) {  // 每1秒输出一次
        printf("Level: %d, Target: %d%%, Current: %d%%, PWM: %s\n",
               g_current_level_index, target_duty_100, g_current_pwm_duty,
               pwm_high_phase ? "HIGH" : "LOW");
    }
}

// 小车后退 - 使用速度控制
void car_backward(void) {
    g_left_motor_enable = 1;
    g_right_motor_enable = 1;
    g_left_motor_direction = 1;   // 反转
    g_right_motor_direction = 1;  // 反转
    //printf("Car backward at speed %d%%\n", g_motor_speed_level);
}

// 小车前进 - 使用速度控制
void car_forward(void) {
    g_left_motor_enable = 1;
    g_right_motor_enable = 1;
    g_left_motor_direction = 0;   // 正转
    g_right_motor_direction = 0;  // 正转
    //printf("Car forward at speed %d%%\n", g_motor_speed_level);
}

// 小车左转 - 使用速度控制
void car_left(void) {
    g_left_motor_enable = 0;      // 左轮停止
    g_right_motor_enable = 1;     // 右轮转动
    g_right_motor_direction = 0;  // 右轮正转
   // printf("Car turn left at speed %d%%\n", g_motor_speed_level);
}

// 小车右转 - 使用速度控制
void car_right(void) {
    g_left_motor_enable = 1;      // 左轮转动
    g_right_motor_enable = 0;     // 右轮停止
    g_left_motor_direction = 0;   // 左轮正转
    //printf("Car turn right at speed %d%%\n", g_motor_speed_level);
}

// 小车停止 - 使用速度控制
void car_stop(void) {
    g_left_motor_enable = 0;      // 左轮停止
    g_right_motor_enable = 0;     // 右轮停止

    // 立即停止所有GPIO输出
    gpio_control(GPIO0, IOT_GPIO_VALUE0);
    gpio_control(GPIO1, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);

    printf("Car stopped\n");
}